// simplified slam for easy maze
//  by Kyohei

#include <ros/ros.h>
#include <Eigen3/Dense>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Pose2D.h>
#include <sensor_msgs/LaserScan.h>
#include "navigation/IR_conv.h"

#include "dgmap.h"

#define FOR(x, a, b) for(typeof(a) x = (a); x < (b); x++)
#define FORIT(x, v) for(typeof(v.begin()) x = v.begin(); x != v.end(); x++)

namespace mzslam
{

class MzSLAM
{
 private:
  ros::Subscriber pose_sub_;
  ros::Subscriber ir_sub_;
  //ros::Subscriber sonar_sub_;
  //ros::Publisher map_pub_;
  //ros::Publisher map_meta_pub_;
  ros::Publisher scan_pub_;

  // robot's pose
  Eigen3::Vector3f x_;
  Eigen3::Vector3f px_; // previous state

  // measurment of walls
  Eigen3::MatrixXf z_;
  Eigen3::MatrixXf pz_; // previous state

  // occupancy grid map
  mzslam::Map map_;

  // constant definition
  enum { FRONT=0, LEFT=1, RIGHT=2, };
  enum { L=0, R=1, };
  enum { F=0, M=1, B=2 };

 public:
  MzSLAM();
  ~MzSLAM();

 protected:
  // [prediction update]
  // update robot's current pose 
  void update_pose(const geometry_msgs::Pose2D::ConstPtr &msg);
  
  // [measument update]
  // register probability on map from ir sensor readings
  void register_walls(const navigation::IR_conv::ConstPtr &msg);

  // estimate wall from ir
  bool estimate_wall(Eigen3::MatrixXf &_ir, int INDEX, Eigen3::Matrix2f &wall, Eigen3::MatrixXf &z);

 private:
  // subfunction to estimate walls
  void _calc_optimized_line(Eigen3::VectorXf &x, Eigen3::VectorXf &y, 
      Eigen3::Vector2f &coeff, float &dist, float &err);
  void _transform_coordinate(Eigen3::Matrix2f &m, int i);

  // receive IR and convert it
  void _convert_IRs(const navigation::IR_conv::ConstPtr &msg, Eigen3::MatrixXf &ir);

  // conversion function to sensor_msgs/LaserScan
  void _convert_wall_to_scan(Eigen3::Matrix2f &wall, sensor_msgs::LaserScan &scan);
  float _calc_scan_ranges(float angle, Eigen3::Matrix2f wall);

  // degree conversion
  inline float _deg2rad(float deg) { return deg*3.14/180.0; }
  inline float _rad2deg(float rad) { return rad/3.14*180.0; }
 
};
} // end namespace
